/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : ekf_uwb_ins.hpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Nov 25, 2020
  ******************************************************************************
  */
/* USER CODE END Header */

#ifndef __EKF_UWB_INS_HPP
#define __EKF_UWB_INS_HPP

#include "system/system.hpp"
#include "userlib/userlib.hpp"


#ifdef __cplusplus

#include <Eigen>

using namespace std;
using namespace Eigen;

#define yaw_cali_window 1000
#define yaw_cali_window_depart 10

class EKFUWB
{
public:
	EKFUWB(){}
	EKFUWB(char *n) : name(n){}
	~EKFUWB(){}
	void EkfUwb_Init();
	void EkfUwb_acc();
	void EkfUwb_Update();
	void EkfUwb_Ins();
	void EkfUwb_YawCali();
	bool EkfUwb_isYawCali();

private:
	char *name;
	sTIM Tim;
	bool Update;
	bool isFirstIns;
	bool isYawCali;
	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;
	uint32_t startTimerLast;
	uint32_t cycleTime_us;

	uint32_t startTimer1;
	uint32_t stopTimer1;
	uint32_t  executionTime_us1;
	uint32_t startTimerLast1;
	uint32_t cycleTime_us1;

	uint32_t startTimer2;
	uint32_t stopTimer2;
	uint32_t  executionTime_us2;

	Quaternion<float> q;


	//矩阵运算变量定义（EKF6）
	Matrix<float,6,9> H;		//灵敏度矩阵
	Matrix<float,9,6> K;		//kalman gain
	Matrix<float,9,9> P_p;		//协方差矩阵预测
	Matrix<float,9,9> P_k;		//协方差矩阵更新
	Matrix<float,6,6> R;		//测量噪声
	Matrix<float,9,9> Q;		//过程噪声
	Matrix<float,6,1> y;					//测量值
	Matrix<float,9,1> X_p;		//状态预测
	Matrix<float,9,1> X_k;		//状态更新
	Matrix<float,9,1> X_k_error;		//状态误差更新
	Matrix<float,9,9> F;
	Matrix<float,9,3> B;
	Matrix<float,9,6> G;
	//-----------------yaw_cali 变量
	Matrix<float,3,yaw_cali_window / yaw_cali_window_depart> source_xyz; //ins坐标
	Matrix<float,3,yaw_cali_window / yaw_cali_window_depart> target_xyz; //uwb坐标
	Matrix<float,3,3> A;
	Matrix<float,3,3> U;
	Matrix<float,3,3> V;
	Matrix<float,3,3> Rotation;
	Vector3f source_temp;
	Vector3f target_temp;
	float lamda;
	float lamda_Ttemp;
	float lamda_temp;
	float yaw_cali;

	Matrix<float,3,3> Cb2n;	//机体坐标系到NED的旋转矩阵

	Vector3f a_k;			//机体坐标系下的加速度
	Vector3f accUWB;
	Matrix3f Rz;			//NED转UWB坐标系旋转矩阵
	Vector3f u_k;			//UWB坐标系下XY轴加速度
	float dt;
	float yaw_mag_record;
	float yaw;

	int YawCaliCnt;

	sensor_acc_msg acc;
	uwb_pos_msg uwbPos;
	ekf_uwb_pos_vel_msg ekf_uwb_pos_vel;
	ahrs_euler_msg ahrsEuler;
	flow_msg flow;
	height_msg height;

};

#endif


#endif


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
